import socket
import threading
import struct
import cv2
import driver
import cmath

HOST = "127.0.0.1"
CAMERA_PORT = 10000
HEAD_PORT = 11000

left_img_bytes = None
right_img_bytes = None

# cv获取视频
def camera():
    cap = cv2.VideoCapture(0)
    # 设置摄像头分辨率
    cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280)
    cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
    while True:
        ret, frame = cap.read()
        left_img = frame[:, 0:640, :]
        right_img = frame[:, 640:1280, :]
        if ret:
            # 显示左摄像头视图
            cv2.imshow('left', left_img)
            # 显示右摄像头视图
            cv2.imshow('right', right_img)

            left_img_bytes = cv2.imencode('.png', left_img)[1].tobytes()
            right_img_bytes = cv2.imencode('.png', right_img)[1].tobytes()

# 发送图像
def sendImg():
    s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)

    while True:
        msg = 'l'.encode('utf-8')
        s.sendto(msg + left_img_bytes, (HOST, CAMERA_PORT))

        msg = 'r'.encode('utf-8')
        s.sendto(msg + right_img_bytes, (HOST, CAMERA_PORT))

# 接收头盔旋转和位置信息
def getHead():
    s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)

    while True:
        s.sendto('1'.encode('utf-8'), (HOST, HEAD_PORT))
        data = s.recvfrom(1024)
        pos_x = struct.unpack('f', data[0][0:4])[0]
        pos_y = struct.unpack('f', data[0][4:8])[0]
        pos_z = struct.unpack('f', data[0][8:12])[0]
        rot_x = struct.unpack('f', data[0][12:16])[0]
        rot_y = struct.unpack('f', data[0][16:20])[0]
        rot_z = struct.unpack('f', data[0][20:24])[0]
        controlCamera(pos_x, pos_y, pos_z, rot_x, rot_y, rot_z)

# 接收手柄信息
def getControl(port):
    s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)

    while True:
        s.sendto('1'.encode('utf-8'), (HOST, port))
        data = s.recvfrom(1024)
        grab = data[0][0] & (1 << 4)
        front = data[0][0] & (1 << 3)
        right = data[0][0] & (1 << 2)
        back = data[0][0] & (1 << 1)
        left = data[0][0] & (1)
        controlDog(port, grab, front, right, back, left)

# 控制云台
trybeg =(0,0,0,0,0)
def controlCamera(pos_x, pos_y, pos_z, rot_x, rot_y, rot_z):
    #这一行需要获得云台传回来的数据，即为beg0至beg4，否则只能应用第一次接收到的输入计算出输出作为beg，后果难以预料。
    end = ()
    length1 = 0.11
    length2 = 0.10 + 0.045 * rot_x      #底座加上上端的高度，决定了中间的长转轴应该扭动的方向。
    pl = 1  #现实与虚拟对应的比例参数。需要根据实际进行调节。
    end[0] = cmath.atan(pos_y/pos_x) # 可能要加90的整倍数.那个具体值需要依靠转轴的上下限制（以及机械臂的固定方式）决定。
    end[1] = cmath.acos(max(1,cmath.sqrt(pos_x*pos_x+ pos_y*pos_y) * pl/length1))         #其中length1约为11。若那个值大于11，则可以算作将脖子伸到最远处。注意，此处默认0对应水平姿态。
    #if pos_z * pl < length2:
    #    end[1] = - ent[1]          #似乎不能存在负数的情况啊…否则对面接收不了这个数据了。
    end[2] = - end[1]       #第一步是保持平行。
    end[3] = rot_y        #与人头的朝向一致。这里大概是要加一个90度的，但也需要测试。
    end[4] += rot_x       #保持仰头（前端可以看成点，大概有1-2厘米的误差。）
    for i in range(0,5):
        setServoPos(i,beg[i],end[i])
        trybeg[i] = end[i]
    #pass

# 控制狗
def controlDog(port, grab, front, right, back, left):
    #那么狗的控制函数呢？ 
    
    #TODO: calculate the angular positions of each servo.
    #We can use setServoPos(channel, beg, end) function to control a servo.
    #Arguments:
    #   channel: id of a servo, from 0 to 4 since we'll have 5 servos.
    #   beg:     original angle of a servo, meaning that we may have an array to store original positions.
    #   end:     ending angle of a servo, which is the angle we let the servo rotate to.
    pass

if __name__ == "__main__":
    threads = []

    threads.append(threading.Thread(target=camera))
    threads.append(threading.Thread(target=sendImg))
    threads.append(threading.Thread(target=getHead))
    threads.append(threading.Thread(target=getControl, args=(12000,)))
    threads.append(threading.Thread(target=getControl, args=(13000,)))

    for t in threads:
        t.start()

    for t in threads:
        t.join()
